package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.command.Command;

public class Input
{
    //Joysticks.
    private Joystick leftJoystick;
    private Joystick rightJoystick;
    private Joystick coDriverJoystick;
    public KinectStick leftArm;
    public KinectStick rightArm;
    private Joystick leftOperJoystick;
    private Joystick rightOperJoystick;
    
    //private JoystickButton trigger;
    //private Command triggerPressed;
    
    //Analog and Digital modules on the robot.
    private AnalogModule anaMod;
    private DigitalModule digiMod;
    
    private DigitalInput ballPosition1;
    private DigitalInput ballPosition2;
    private DigitalInput ballPosition3;
    private DigitalInput ramp1;
    private DigitalInput ramp2;
    
    //Constants for getAxis.
    static final public int xAxis = 0;
    static final public int yAxis = 1;
    static final public int zAxis = 2;
    static final public int rightStick = 3;
    static final public int leftStick = 4;
    static final public int coDriverStick = 5;
    
    public Input() {
        //Initialize the joysticks and modules with their respective ports.
        this.leftJoystick = new Joystick(1);
        this.rightJoystick = new Joystick(2);
        this.coDriverJoystick = new Joystick(3);
        this.anaMod = AnalogModule.getInstance(1);
        this.digiMod = DigitalModule.getInstance(1); 
        this.ballPosition1 = new DigitalInput(1);
        this.ballPosition2 = new DigitalInput(2);
        this.ballPosition3 = new DigitalInput(3);
        this.ramp1 = new DigitalInput(6);
        this.ramp2 = new DigitalInput(7);
    }
    
    public int getRange() {
        //Return the value of the range sensor.
        return anaMod.getAverageValue(1);
    }
    
    
    public double getAxis(int joystick, int axis) {
        //Returns the value of the given joystick's given axis.
        Joystick curJoystick;
        switch(joystick)
        {
            case rightStick:
                curJoystick = this.rightJoystick;
                break;
            case leftStick:
                curJoystick = this.leftJoystick;
                break;
            case coDriverStick:
                curJoystick = this.coDriverJoystick;
                break;
            default:
                System.out.println("Unexpected joystick value");
                return -1;
                
        }
        
        switch(axis)
        {
            case xAxis:
                return curJoystick.getX() *-1;
            case yAxis:
                return curJoystick.getY() *-1;
            case zAxis:
                return curJoystick.getZ() *-1;
            default:
                System.out.println("Unexpected joystick axis value");
                return -1;
        }   
    }
    public int getKinectState() {
        if (leftArm.getY() > 0.1 && rightArm.getY() > 0.1) {
            //If both arms are up
            return 1;     
        } else if (leftArm.getY() < -0.1 && rightArm.getY() < -0.1) {
            //If both arms are down
            return 2;
        } else if (leftArm.getY() > 0.1 && rightArm.getY() < -0.1) {
            //Left arm up, right arm down
            return 3;
        } else if (leftArm.getY() < -0.1 && rightArm.getY() > 0.1) {
            //Left arm down, right arm up
            return 4;
        } else { 
            return 0;
        }        
    }
    public double getThrottle(int stick){
        //Returns the value of the given joystick's throttle
        Joystick currentStick;
        switch(stick)
        {
            case rightStick:
                currentStick = this.rightJoystick;
                break;
            case leftStick:
                currentStick = this.leftJoystick;
            case coDriverStick:
                currentStick = this.coDriverJoystick;
                break;

            default:
                System.out.println("Unexpected joystick value");
                return 0.0;
        }
        return currentStick.getThrottle();
   }
    
    public boolean getStickButtonState(int stick, int button) {
        //Returns the value of the given joystick's given axis.
        Joystick currentStick;
        switch(stick)
        {
            case rightStick:
                currentStick = this.rightJoystick;
                break;
            case leftStick:
                currentStick = this.leftJoystick;
                break;
            case coDriverStick:
                currentStick = this.coDriverJoystick;
                break;

            default:
                System.out.println("Unexpected joystick value");
                return false;
        }
        return currentStick.getRawButton(button);
    }
    
    public boolean getStickTrigger(int stick) {
        Joystick currentStick;
        switch(stick)
        {
            case rightStick:
                currentStick = this.rightJoystick;
                break;
            case leftStick:
                currentStick = this.leftJoystick;
                break;
            default:
                System.out.println("Unexpected joystick value");
                return false;
        }
        
        //Command c = JoystickButton(currentStick,1);
        return currentStick.getTrigger();
        
    }
    public int getLeftDistance(){
        return this.anaMod.getAverageValue(2);
    }
    public int getRightDistamce(){
        return this.anaMod.getAverageValue(3);
    }
    //Commented this out because a less complex method was written.
    /*public boolean[] getBallPosition() {
        boolean[] returns = {};
        returns[0] = ballPosition1.get(); 
        returns[1] = ballPosition2.get();
        returns[2] = ballPosition3.get();
        return returns;
    }
    * 
    */
    public boolean getInput(int id)
    {
       boolean bSensorState = false;
       switch(id)
       {
           case 1:
               bSensorState = ballPosition1.get();
               break;
           case 2:
               bSensorState = ballPosition2.get();
               break;
           case 3:
               bSensorState = ballPosition3.get();
               break;
           default:
               break;
       }
       return bSensorState;
    }
    public boolean getRamp1()
    {
        return this.ramp1.get();
    }
    public boolean getRamp2()
    {
        return this.ramp2.get();
    }
	
	/**
	 * Get the voltage from the potentiometer
	 * @return Voltage from analog module
	 */
	public double getAngleVoltage() {
		return this.anaMod.getAverageVoltage(1);
	}
}
